#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=5118;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

Eigen::Matrix4f P2_, Trv2c_;

void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string& out_file)
{
    //Create & write .bin file
//    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
    ofstream bin_file(out_file.c_str());
    if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

    //PCD 2 BIN
    cout << "Converting "<<endl;
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
        bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
        //cout<< 	cloud->points[i]<<endl;
    }

    bin_file.close();
}

void pc_range_handle(pcl::PointCloud<PointType>::Ptr p_pc){
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*p_pc, *p_pc, indices);
    pcl::PassThrough<PointType> pass_filter;
    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("z");
    pass_filter.setFilterLimits (-1.2, 0);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);

    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("y");
    pass_filter.setFilterLimits (-10,10);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);

    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("x");
    pass_filter.setFilterLimits (0, 20);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);
}


void cloudCB( vector<string> pc_path)
{
    for(int label_idx = 0; label_idx < pc_path.size(); label_idx++){
        frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                        "%06d", frame_num_));
        pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
        pcl::io::loadPCDFile<PointType>(pc_path[label_idx], *pc_tmp);

        pc_range_handle(pc_tmp);
    
        string tranin_or_test="training";
        // string tranin_or_test="testing";
        string dataset_name = "1119";
        cout<<tranin_or_test<<endl;

        for(auto & pt:pc_tmp->points){
            pt.intensity = 0;
        }
        pc_path_name_="/home/st/ubuntu_data/dataset/nrsl/tj/"+dataset_name+"/"+tranin_or_test+"/velodyne_cut/"+frame_id_string_+".bin";
        pcd2bin(pc_tmp,pc_path_name_);
        frame_num_++;
    }
}

vector<string> getFiles(string cate_dir)
{
    vector<string> files; //存放文件名

#ifdef linux
    DIR *dir;
    struct dirent *ptr;
    char base[1000];

    if ((dir = opendir(cate_dir.c_str())) == NULL)
    {
        perror("Open dir error...");
        exit(1);
    }

    while ((ptr=readdir(dir)) != NULL)
	{
		if(strcmp(ptr->d_name,".")==0 || strcmp(ptr->d_name,"..")==0)    ///current dir OR parrent dir
            continue;
        else if(ptr->d_type == 8)    ///file
			//printf("d_name:%s/%s\n",basePath,ptr->d_name);
			files.push_back(ptr->d_name);
		else if(ptr->d_type == 10)    ///link file
			//printf("d_name:%s/%s\n",basePath,ptr->d_name);
			continue;
		else if(ptr->d_type == 4)    ///dir
		{
			files.push_back(ptr->d_name);
			/*
		        memset(base,'\0',sizeof(base));
		        strcpy(base,basePath);
		        strcat(base,"/");
		        strcat(base,ptr->d_nSame);
		        readFileList(base);
			*/
		}
	}
	closedir(dir);
#endif
 
	//排序，按从小到大排序
	sort(files.begin(), files.end());
	return files;
}

int main(int argc, char* argv[])
{
    vector<string> pc_path = getFiles("/home/st/ubuntu_data/dataset/nrsl/tj/1119/training/velodyne");
    
    for (auto &path : pc_path){
        path = "/home/st/ubuntu_data/dataset/nrsl/tj/1119/training/velodyne/"+path;
        cout<<path<<endl;
    }


    cloudCB(pc_path);

    return 0;
}
